
#include <std.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <idl.h>
#include <sem.h>
#include <tsk.h>
#include <ecm.h>
#include <log.h>
#include <swi.h>
#include <gio.h>
#include <gbl.h>
#include <clk.h>
#include <que.h>
#include "matrix.h"


/* ========================================================================== */
/*                           FUNCTION DEFINITIONS                             */
/* ========================================================================== */

/* \fn
 *
 * \brief   Math Library - Matrix operation
 *
 */





/* Square Matrix Inverse Function
 * N - dimension of square matrix
 * mat1 - matrix A
 * mat2 - matrix A Inverse
 * Reture 0 on success, -1 on failure
 * */
int MatInverse(int N,  double mat1[], double mat2[])
{
   double y, eps = 1e-20;
   int i, j, k, mj, mk, kk;

  //The array definition below is illegal according to standard C/C++ grammar.
  //It is suprised that it is workable with Nios2 compiler. It avoids to use
  //dynamic memory allocation such as new-delete pair.
  //Yong Li, SNAPlab/UNSW, 4May2007
  // float *b = new float[N];
  // float *p = new float[N*N];
  // float *q = new float[N*N];
   double *b = MEM_alloc(0,N*sizeof(double),1); //allocate from Seg0 and align
   double *p = MEM_alloc(0,N*N*sizeof(double),1); //allocate from Seg0 and align
   double *q = MEM_alloc(0,N*N*sizeof(double),1); //allocate from Seg0 and align


   for(i=0;i<N*N;i++)
    mat2[i] = mat1[i];
   for(i=0; i<N; i++)
    for(j=0; j<N; j++)
    {
      p[i*N+j] = 0;
      if(i==j) p[i*N+j] = 1;
      q[i*N+j] = p[i*N+j];
    }

   for(i=0; i<N; i++)
   {
      y=0;
      for(j=i; j<N; j++)
        for(k=i; k<N; k++)
          if(fabsf(mat2[j*N+k]) > fabsf(y))
          { mj = j, mk = k, y = mat2[mj*N+mk]; }

      if(mk!=i)
      for(k=0; k<N; k++)
      {
        b[k] = mat2[k*N+mk];
        mat2[k*N+mk] = mat2[k*N+i];
        mat2[k*N+i] = b[k];
        b[k] = q[k*N+mk];
        q[k*N+mk] = q[k*N+i];
        q[k*N+i] = b[k];
      }

      if(mj!=i)
        for(k=0; k<N; k++)
        {
          b[k] = mat2[mj*N+k];
          mat2[mj*N+k] = mat2[i*N+k];
          mat2[i*N+k] = b[k];
          b[k] = p[mj*N+k];
          p[mj*N+k] = p[i*N+k];
          p[i*N+k] = b[k];
        }


      if(fabsf(mat2[i*N+i]) < eps) // Can't continue any more !
      {
    	  MEM_free(0,b,N*sizeof(double)); //put it back to the mem pool
    	  MEM_free(0,p,N*N*sizeof(double)); //put it back to the mem pool
    	  MEM_free(0,q,N*N*sizeof(double)); //put it back to the mem pool
    	  return -1;
      }


      for(j=i+1; j<N; j++)
      {
        b[j] = mat2[j*N+i];
        for(k=0; k<N; k++)
        {
          mat2[j*N+k] -= b[j]/mat2[i*N+i]*mat2[i*N+k];
          p[j*N+k] -= b[j]/mat2[i*N+i]*p[i*N+k];
        }
        b[j] = mat2[i*N+j];
        for(k=0; k<N; k++)
        {
          mat2[k*N+j] -= b[j]/mat2[i*N+i]*mat2[k*N+i];
          q[k*N+j] -= b[j]/mat2[i*N+i]*q[k*N+i];
        }
      }

      for(kk=0; kk<N; kk++)
        p[i*N+kk] /= mat2[i*N+i];
   }

   for(i=0; i<N; i++)
    for(j=0; j<N; j++)
    {
      mat2[i*N+j] = 0;
      for(k=0; k<N; k++)
        mat2[i*N+j] += q[i*N+k]*p[k*N+j];
    }

  //delete b,p,q;
  MEM_free(0,b,N*sizeof(double)); //put it back to the mem pool
  MEM_free(0,p,N*N*sizeof(double)); //put it back to the mem pool
  MEM_free(0,q,N*N*sizeof(double)); //put it back to the mem pool
  return 0;
}


double  VecNorm(int dim, double vec[])
{
	double norm2 = 0;
    int i = 0;
    for(i=0; i<dim;i++)
    	norm2 += (vec[i]*vec[i]);
    return sqrtf(norm2);
}

double  VecDotMul(int dim, double vec1[], double vec2[])
//input: dim, vec1[dim], vec2[dim]
//output:
//operator: =[vec1]_t . [vec2]
{

	double dotValue=0;
  int i = 0;
  for(i=0;i<dim;i++)
     dotValue += vec1[i]*vec2[i];

  return dotValue;
}

void  VecToCrossMulMat(double vec[], double mat[])
//input:  vec[3]
//output: mat[3*3]
//operator: mat = vecX
{
   mat[0*3+0] = 0;      mat[0*3+1]=-vec[2]; mat[0*3+2]=vec[1];
   mat[1*3+0] = vec[2]; mat[1*3+1]= 0;      mat[1*3+2]=-vec[0];
   mat[2*3+0] =-vec[1]; mat[2*3+1]= vec[0]; mat[2*3+2]=0;
}

void  VecCross(double vec1[], double vec2[], double vec3[])

{
  vec3[0] = vec1[1]*vec2[2] - vec1[2]*vec2[1];
  vec3[1] = vec1[2]*vec2[0] - vec1[0]*vec2[2];
  vec3[2] = vec1[0]*vec2[1] - vec1[1]*vec2[0];
}

void  TwoVec_OrthogMatrix(double x1[3], double x2[3], double M[3*3])
{
	double u[3], v[3], w[3];
	double x3[3]; //x3 = x1 x x2
  int j = 0;
  MatAdd(3,1,x1,x2,x3); //x3 = x1+x2
  VecUnit(3,x3,u);//u=unit(x1+x2)
  MatMinusf(3,1,x1,x2,x3);//x3=x1-x2
  VecUnit(3,x3,v);//v=unit(x1-x2)
  VecCross(u,v,x3);
  VecUnit(3,x3,v);
  VecCross(u,v,w);

  for(j=0; j<3; j++)
    {
      M[j*3+0] = u[j];
      M[j*3+1] = v[j];
      M[j*3+2] = w[j];
    }
}

void  VecUnit(int dim, double vec1[], double vec2[])
//input: dim, vec1[dim]
//output: vec2[dim]
//operator: vec2 = vec1 / norm(vec1)
{
  int i;
  double vec1Norm = VecNorm(dim, vec1);
  if(vec1Norm<1e-30)
    MatEqualf(dim,1,vec1,vec2);
  else
  {
    for(i=0; i<dim; i++)
      vec2[i] = vec1[i]/vec1Norm;
  }
}

void  MatZero(int row, int column, double mat[])
{
  int i,j;
  double zero = 0.0f;
  for(i=0; i<row; i++)
    for(j=0; j<column; j++)
      mat[i*column+j] = zero;
}

void  MatMulScalar(int row,int column,double mat1[],double scalar,double mat2[])
{
  int i, j;
  for(i=0; i<row; i++)
    for(j=0; j<column; j++)
     mat2[i*column+j] = mat1[i*column+j]*scalar;
}

void  MatMul(int row, int middle, int column,double mat1[], double mat2[], double mat3[])
{
   int i, j, k;
   for(i=0; i<row; i++)
   {
      for(j=0; j<column; j++)
      {
       mat3[i*column+j] = 0.;
       for(k=0; k<middle; k++)
          mat3[i*column+j] += mat1[i*middle+k] * mat2[k*column+j];
      }
   }
 }

void  MatEquald(int row, int column, double mat1[], double mat2[])
{
  memcpy(mat2,mat1,sizeof(double)*row*column);
}

void  MatEqualf(int row, int column, double mat1[], double mat2[])
{
  memcpy(mat2,mat1,sizeof(double)*row*column);
}

void  MatAdd(int row, int column, double mat1[], double mat2[], double mat3[])
{
 int i, j;
 for(i=0; i<row; i++)
  for(j=0; j<column; j++)
      mat3[i*column+j] = mat2[i*column+j] + mat1[i*column+j];
}

void  MatMinusd(int row, int column, double mat1[], double mat2[], double mat3[])
{
 int i, j;
 for(i=0; i<row; i++)
  for(j=0; j<column; j++)
      mat3[i*column+j] = mat1[i*column+j] - mat2[i*column+j];
}

void  MatMinusf(int row, int column, double mat1[], double mat2[], double mat3[])
{
 int i, j;
 for(i=0; i<row; i++)
  for(j=0; j<column; j++)
      mat3[i*column+j] = mat1[i*column+j] - mat2[i*column+j];
}

void  MatTrans(int row, int column, double mat1[], double mat2[])
{
 int i, j;
 for(i=0; i<row; i++)
  for(j=0; j<column; j++)
    mat2[j*row+i] = mat1[i*column+j];
}

void  MatUnit(int N,  double mat[])
{
  int i,j;
  for(i=0; i<N; i++)
    for(j=0; j<N; j++)
  {
    mat[i*N+j] = 0.;
    if(i==j)
    mat[i*N+j] = 1.;
  }
}

void MatSetBlock(int row,int col, int StartRow,int StartCol,int EndRow, int EndCol, double BlockMat[],double Mat[])
//intput:   row,m
//intput:  (StartRow, StartCol)and (EndRow, EndCol) define a black
//intput:  BlackMat[(EndRow-StartRow+1)*(EndCol-StartCol+1)]
//output:  Mat[row*col]
{
  int i,j;
  if(EndRow>row || EndCol>col)
   return;

  int BlockRow=EndRow-StartRow+1;
  int BlockCol=EndCol-StartCol+1;

  for(i=0; i<BlockRow;i++)
    for(j=0; j<BlockCol;j++)
      Mat[(i+StartRow)*col+(j+StartCol)]=BlockMat[i*BlockCol+j];
}


/* ========================================================================== */
/*                                END OF FILE                                 */
/* ========================================================================== */
